#ifndef _MOTOR_H_
#define _MOTOR_H_
#include "pwm.h"
#include "buttons.h"
#include "adc.h"

unsigned char MOTOR_running = 0;

#define MOTOR_PWM_DDR DDRB
#define MOTOR_PWM_PORT PORTB

#define MOTOR_ENABLE 2

#define MOTOR_INPUT_DDR DDRD
#define MOTOR_INPUT_PORT PORTD

#define MOTOR_LEFT 2
#define MOTOR_RIGHT 3

#define STOP 				100
#define GOLEFT 				'L' //76
#define GORIGHT				'P' //80
#define CHANGE_DIRECTION 	102
#define START				103

//Zezwolenie na start
extern volatile unsigned char Zezwolenie;
inline void MOTOR_init()
{
	//MOTOR_PWM jako wyjscie
	MOTOR_PWM_DDR |= (1<<MOTOR_ENABLE);
	//MOTOR_INPUT jako wyjscie => polaczenie z driverem
	MOTOR_INPUT_DDR |= (1<<MOTOR_LEFT) | (1<<MOTOR_RIGHT);

	MOTOR_PWM_PORT &= ~(1<<MOTOR_ENABLE);

	MOTOR_INPUT_PORT |= (1<<MOTOR_LEFT);
	MOTOR_INPUT_PORT &= ~(1<<MOTOR_RIGHT);
	
	PWM_init();
	MOTOR_running = 0;
}

void MOTOR_command(unsigned char command)
{
	switch(command)
	{
		case GOLEFT: 
			MOTOR_INPUT_PORT |= (1<<MOTOR_LEFT);
			MOTOR_INPUT_PORT &= ~(1<<MOTOR_RIGHT);
			break;
		case GORIGHT:
			MOTOR_INPUT_PORT |= (1<<MOTOR_RIGHT);
			MOTOR_INPUT_PORT &= ~(1<<MOTOR_LEFT);
			break;
		case CHANGE_DIRECTION:
			MOTOR_INPUT_PORT ^= (1<<MOTOR_LEFT);
			MOTOR_INPUT_PORT ^= (1<<MOTOR_RIGHT);
			break;
		case STOP:
			MOTOR_running = 0;
			break;
		case START:
			MOTOR_running = 1;
			break;
	};
}

inline void MOTOR_speed()
{
	if (STEROWANIE_KOMPUTEREM_ison())
	{
		if (Zezwolenie == '1' && MOTOR_running == 1)
			PWM_fill(Predkosc);
			else
			PWM_fill(0);
	}
	else
	{
		if (ZEZWOLENIE_ison() && MOTOR_running == 1)
			PWM_fill(ADC_value);
			else
			PWM_fill(0);
	}
}

#endif
